{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "8e38905f",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "51022d47",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "import copy  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "da79b9bc",
   "metadata": {},
   "outputs": [],
   "source": [
    "def read_ply_files(directory):\n",
    "    \"\"\"读取目录中所有的PLY文件\"\"\"\n",
    "    ply_files = []\n",
    "    for filename in os.listdir(directory):\n",
    "        if filename.endswith(\".ply\"):\n",
    "            filepath = os.path.join(directory, filename)\n",
    "            pcd = o3d.io.read_point_cloud(filepath)\n",
    "            ply_files.append((filename, pcd))\n",
    "    return ply_files\n",
    "\n",
    "\n",
    "def preprocess_point_cloud(pcd, voxel_size):\n",
    "    \"\"\"点云预处理：下采样、计算法线和特征\"\"\"\n",
    "    pcd_down = pcd.voxel_down_sample(voxel_size)\n",
    "    pcd_down.estimate_normals(\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2, max_nn=30)\n",
    "    )\n",
    "    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(\n",
    "        pcd_down,\n",
    "        o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5, max_nn=100),\n",
    "    )\n",
    "    return pcd_down, pcd_fpfh\n",
    "\n",
    "\n",
    "def execute_global_registration(\n",
    "    source_down, target_down, source_fpfh, target_fpfh, voxel_size\n",
    "):\n",
    "    \"\"\"执行全局配准，获取初始变换矩阵\"\"\"\n",
    "    distance_threshold = voxel_size * 1.5\n",
    "    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(\n",
    "        source_down,\n",
    "        target_down,\n",
    "        source_fpfh,\n",
    "        target_fpfh,\n",
    "        True,\n",
    "        distance_threshold,\n",
    "        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),\n",
    "        3,\n",
    "        [\n",
    "            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),\n",
    "            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(\n",
    "                distance_threshold\n",
    "            ),\n",
    "        ],\n",
    "        o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999),\n",
    "    )\n",
    "    return result\n",
    "\n",
    "\n",
    "def execute_icp_registration(source, target, init_transformation, threshold):\n",
    "    \"\"\"执行ICP精配准\"\"\"\n",
    "    reg_p2p = o3d.pipelines.registration.registration_icp(\n",
    "        source,\n",
    "        target,\n",
    "        threshold,\n",
    "        init_transformation,\n",
    "        o3d.pipelines.registration.TransformationEstimationPointToPoint(),\n",
    "    )\n",
    "    return reg_p2p\n",
    "\n",
    "\n",
    "def visualize_registration(source, target, transformation):\n",
    "    \"\"\"可视化配准结果\"\"\"\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])  # 源点云设为黄色\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])  # 目标点云设为蓝色\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target_temp])\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "b6ffc20d",
   "metadata": {},
   "outputs": [],
   "source": [
    "def main():\n",
    "    # 设置参数\n",
    "    directory = \"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\"  # 存放PLY文件的目录\n",
    "    voxel_size = 0.05  # 下采样体素大小\n",
    "    icp_threshold = 0.02  # ICP距离阈值\n",
    "\n",
    "    # 读取点云文件\n",
    "    ply_files = read_ply_files(directory)\n",
    "    if len(ply_files) < 2:\n",
    "        print(\"需要至少两个点云文件进行配准\")\n",
    "        return\n",
    "\n",
    "    # 选择第一个点云作为参考\n",
    "    reference_name, reference_pcd = ply_files[0]\n",
    "    print(f\"参考点云: {reference_name}\")\n",
    "\n",
    "    # 处理并配准其他点云\n",
    "    for i in range(1, len(ply_files)):\n",
    "        target_name, target_pcd = ply_files[i]\n",
    "        print(f\"正在配准: {target_name}\")\n",
    "\n",
    "        # 预处理点云\n",
    "        source_down, source_fpfh = preprocess_point_cloud(reference_pcd, voxel_size)\n",
    "        target_down, target_fpfh = preprocess_point_cloud(target_pcd, voxel_size)\n",
    "\n",
    "        # 全局配准获取初始变换\n",
    "        result_ransac = execute_global_registration(\n",
    "            source_down, target_down, source_fpfh, target_fpfh, voxel_size\n",
    "        )\n",
    "\n",
    "        # ICP精配准\n",
    "        result_icp = execute_icp_registration(\n",
    "            source_down, target_down, result_ransac.transformation, icp_threshold\n",
    "        )\n",
    "\n",
    "        # 应用变换到原始点云\n",
    "        final_transformation = result_icp.transformation\n",
    "        transformed_pcd = target_pcd.transform(final_transformation)\n",
    "\n",
    "        # 保存配准结果\n",
    "        output_path = os.path.join(directory, f\"aligned_{target_name}\")\n",
    "        o3d.io.write_point_cloud(output_path, transformed_pcd)\n",
    "        print(f\"已保存配准结果: {output_path}\")\n",
    "\n",
    "        # 可视化配准结果\n",
    "        visualize_registration(reference_pcd, target_pcd, final_transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "bd6e72e0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "参考点云: Image_Cloud_20241108_15_39_19_379.ply\n",
      "正在配准: Image_Cloud_20241108_15_39_59_904.ply\n",
      "已保存配准结果: C:\\Users\\Henan\\Downloads\\Part1_test_ICP\\aligned_Image_Cloud_20241108_15_39_59_904.ply\n"
     ]
    },
    {
     "ename": "NameError",
     "evalue": "name 'copy' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[31m---------------------------------------------------------------------------\u001b[39m",
      "\u001b[31mNameError\u001b[39m                                 Traceback (most recent call last)",
      "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[5]\u001b[39m\u001b[32m, line 2\u001b[39m\n\u001b[32m      1\u001b[39m \u001b[38;5;28;01mif\u001b[39;00m \u001b[34m__name__\u001b[39m == \u001b[33m\"\u001b[39m\u001b[33m__main__\u001b[39m\u001b[33m\"\u001b[39m:\n\u001b[32m----> \u001b[39m\u001b[32m2\u001b[39m     \u001b[43mmain\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\n",
      "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[4]\u001b[39m\u001b[32m, line 46\u001b[39m, in \u001b[36mmain\u001b[39m\u001b[34m()\u001b[39m\n\u001b[32m     43\u001b[39m \u001b[38;5;28mprint\u001b[39m(\u001b[33mf\u001b[39m\u001b[33m\"\u001b[39m\u001b[33m已保存配准结果: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00moutput_path\u001b[38;5;132;01m}\u001b[39;00m\u001b[33m\"\u001b[39m)\n\u001b[32m     45\u001b[39m \u001b[38;5;66;03m# 可视化配准结果\u001b[39;00m\n\u001b[32m---> \u001b[39m\u001b[32m46\u001b[39m \u001b[43mvisualize_registration\u001b[49m\u001b[43m(\u001b[49m\u001b[43mreference_pcd\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mtarget_pcd\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mfinal_transformation\u001b[49m\u001b[43m)\u001b[49m\n",
      "\u001b[36mCell\u001b[39m\u001b[36m \u001b[39m\u001b[32mIn[2]\u001b[39m\u001b[32m, line 64\u001b[39m, in \u001b[36mvisualize_registration\u001b[39m\u001b[34m(source, target, transformation)\u001b[39m\n\u001b[32m     62\u001b[39m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[34mvisualize_registration\u001b[39m(source, target, transformation):\n\u001b[32m     63\u001b[39m \u001b[38;5;250m    \u001b[39m\u001b[33;03m\"\"\"可视化配准结果\"\"\"\u001b[39;00m\n\u001b[32m---> \u001b[39m\u001b[32m64\u001b[39m     source_temp = \u001b[43mcopy\u001b[49m.deepcopy(source)\n\u001b[32m     65\u001b[39m     target_temp = copy.deepcopy(target)\n\u001b[32m     66\u001b[39m     source_temp.paint_uniform_color([\u001b[32m1\u001b[39m, \u001b[32m0.706\u001b[39m, \u001b[32m0\u001b[39m])  \u001b[38;5;66;03m# 源点云设为黄色\u001b[39;00m\n",
      "\u001b[31mNameError\u001b[39m: name 'copy' is not defined"
     ]
    }
   ],
   "source": [
    "if __name__ == \"__main__\":\n",
    "    main()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "19d27637",
   "metadata": {},
   "source": [
    "单独进行可视化。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "a25f3d62",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# 替换为你的文件路径\n",
    "reference_path = \"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\Image_Cloud_20241108_15_39_19_379.ply\"  # 参考点云\n",
    "aligned_path = \"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\aligned_Image_Cloud_20241108_15_39_59_904.ply\"    # 已配准的目标点云\n",
    "\n",
    "# 读取点云\n",
    "reference_pcd = o3d.io.read_point_cloud(reference_path)\n",
    "aligned_pcd = o3d.io.read_point_cloud(aligned_path)\n",
    "\n",
    "# 假设 aligned_pcd 已经应用了变换，我们需要反向变换来获取变换矩阵\n",
    "# 注意：这种方法仅适用于已保存的对齐结果\n",
    "identity_matrix = np.eye(4)  # 单位矩阵\n",
    "visualize_registration(reference_pcd, aligned_pcd, identity_matrix)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bad2be14",
   "metadata": {},
   "source": [
    "对齐指标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2685c308",
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_registration_error(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    source_temp.transform(transformation)\n",
    "    distances = source_temp.compute_point_cloud_distance(target)\n",
    "    # MSE 去开平方\n",
    "    # return np.mean(distances)\n",
    "    return np.sqrt(np.mean(distances))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d46993d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 从文件加载点云\n",
    "reference_pcd = o3d.io.read_point_cloud(\"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\Image_Cloud_20241108_15_39_19_379.ply\")\n",
    "aligned_pcd = o3d.io.read_point_cloud(\"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\aligned_Image_Cloud_20241108_15_39_59_904.ply\")\n",
    "\n",
    "# 计算已对齐点云与参考点云之间的误差\n",
    "error = compute_registration_error(aligned_pcd, reference_pcd, np.eye(4))\n",
    "print(f\"配准误差: {error:.6f}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "22de7286",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "原始点云点数: 11919936\n",
      "下采样后点数: 10194858\n",
      "配准误差: 0.269358\n"
     ]
    }
   ],
   "source": [
    "# 从文件加载点云\n",
    "reference_pcd = o3d.io.read_point_cloud(\"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\Image_Cloud_20241108_15_39_19_379.ply\")\n",
    "aligned_pcd = o3d.io.read_point_cloud(\"C:\\\\Users\\\\Henan\\\\Downloads\\\\Part1_test_ICP\\\\aligned_Image_Cloud_20241108_15_39_59_904.ply\")\n",
    "\n",
    "# 下采样点云\n",
    "voxel_size = 0.01  # 根据你的数据调整体素大小\n",
    "reference_down = reference_pcd.voxel_down_sample(voxel_size)\n",
    "aligned_down = aligned_pcd.voxel_down_sample(voxel_size)\n",
    "\n",
    "print(f\"原始点云点数: {len(reference_pcd.points)}\")\n",
    "print(f\"下采样后点数: {len(reference_down.points)}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21d3def7",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# 计算下采样点云之间的误差\n",
    "error = compute_registration_error(aligned_down, reference_down, np.eye(4))\n",
    "print(f\"配准误差: {error:.6f}\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "my_env",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
